#include "include/KalmanFilters.h"

using namespace Bayesian_filter;
using namespace Bayesian_filter_matrix;
using namespace Ei;
using namespace Ogre;


// Constant Dimensions
const unsigned NX = 2;			// Filter State dimension 	(Position, Velocity)

// Filter Parameters
// Prediction parameters for Integrated Ornstein-Uhlembeck Process
const Float dt = .8;
const Float V_NOISE = 0.001;	// Velocity noise, giving mean squared error bound
const Float V_GAMMA = .1;	// Velocity correlation, giving velocity change time constant
// Filter's Initial state uncertainty: System state is unknown
const Float i_P_NOISE = .01;	
const Float i_V_NOISE = .01;
// Noise on observing system state
const Float OBS_INTERVAL = 0.10;
const Float OBS_NOISE = 0.0001;



//
// Predict Model
//
PVpredict::PVpredict() : Linear_predict_model(NX, 1)
{
	// Position Velocity dependance
	const Float Fvv = exp(-dt*V_GAMMA);
	Fx(0,0) = 1.;
	Fx(0,1) = dt;
	Fx(1,0) = 0.;
	Fx(1,1) = Fvv;
	// Setup constant noise model: G is identity
	q[0] = dt*sqr((1-Fvv)*V_NOISE);
	G(0,0) = 0.;
	G(1,0) = 1.;
}

//
// observeModel
//
PVobserve::PVobserve () :
	Linrz_uncorrelated_observe_model(NX,1), z_pred(1)
{
	// Linear model
	Hx(0,0) = 1;
	Hx(0,1) = 0.;
	// Observation Noise variance
	Zv[0] = sqr(OBS_NOISE);
}


//
// Full Filter
//
VelocityInterpolator::VelocityInterpolator() : mDirectFilter(NX,NX) //// use NX-size filter
{
	// intialise the direct and indirect kalman filter
	Vec x_true (NX);

	// True State to be observed
	x_true[0] = 1.;	// Position
	x_true[1] = 0.;	// Velocity
 
	// Construct Prediction and Observation model and filter
	// Give the filter an initial guess of the system state
	Vec x_guess(NX);
	x_guess[0] = -1.;
	x_guess[1] = 0;

	// mDirectFilter filter construct and initialize with initial state guess
	mDirectFilter.X.clear();
	mDirectFilter.X(0,0) = sqr(i_P_NOISE);
	mDirectFilter.X(1,1) = sqr(i_V_NOISE);
		
	mDirectFilter.init_kalman (x_guess, mDirectFilter.X);
	
	
}

VelocityInterpolator::~VelocityInterpolator()
{

}
	
Real VelocityInterpolator::Predict()
{
	mDirectFilter.predict(mPredict);
	mDirectFilter.update();
		
	Vec v(1);	
	v = mDirectFilter.x; 

	return Real(v[0]);

}

void VelocityInterpolator::Measure(Real data)
{
	// Make an observation
	Vec z(1);	
	z[0] = data;
	mDirectFilter.observe(mObserve, z);
	mDirectFilter.update();

}

void VelocityInterpolator::init()
{	
// Initialise state guess and covarince


}
